/*
Copyright (c) 2017, ChanYuan KUO, YoRu LU,
latest editor: HaoChih, LIN
All rights reserved. (Hypha ROS Workshop)

This file is part of hypha_racecar package.

hypha_racecar is free software: you can redistribute it and/or modify
it under the terms of the GNU LESSER GENERAL PUBLIC LICENSE as published
by the Free Software Foundation, either version 3 of the License, or
any later version.

hypha_racecar is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU LESSER GENERAL PUBLIC LICENSE for more details.

You should have received a copy of the GNU LESSER GENERAL PUBLIC LICENSE
along with hypha_racecar.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include "ros/ros.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include "nav_msgs/Path.h"
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>

#define PI 3.14159265358979

/********************/
/* CLASS DEFINITION */
/********************/
class L1Controller
{
    public:
        L1Controller();
        void initMarker();
        bool isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose);
        bool isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos);
        double getYawFromPose(const geometry_msgs::Pose& carPose);     
        double getVecAngle(const geometry_msgs::Point& Vec1, const geometry_msgs::Point& Vec2);
        double getEta(const geometry_msgs::Pose& carPose);
        double getL1Distance(const double& _Vcmd);
        double getSteeringAngle(double eta);
        double getGasInput(const float& current_v);
        geometry_msgs::Point get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose);

    private:
        ros::NodeHandle n_;
        ros::Subscriber odom_sub, path_sub, goal_sub,goal_reached_sub;
        ros::Publisher pub_, marker_pub,goal_reached_pub;
        ros::Timer timer1, timer2;
        tf::TransformListener tf_listener;

        visualization_msgs::Marker points, line_strip, goal_circle;
        geometry_msgs::Twist cmd_vel;
        geometry_msgs::Point odom_goal_pos,goal_reached_pub_point;
        nav_msgs::Odometry odom;
        nav_msgs::Path map_path, odom_path;

        double L, Lfw, Lrv, Vcmd, lfw, lrv, steering, u, v;
        double Gas_gain, baseAngle, Angle_gain_P,Angle_gain_D, goalRadius,brandrate,min_speed;
        int controller_freq;
        double baseSpeed,test_rate;
        int  forword_point;
        bool foundForwardPt, goal_received, goal_reached;

        void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);
        void pathCB(const nav_msgs::Path::ConstPtr& pathMsg);
        void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg);
        void goalReachingCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& goal_msg);
        void controlLoopCB(const ros::TimerEvent&);

}; // end of class


L1Controller::L1Controller()
{
    //Private parameters handler
    ros::NodeHandle pn("~");

    //Car parameter
    pn.param("L", L, 0.26);
    pn.param("Lrv", Lrv, 10.0);
    pn.param("Vcmd", Vcmd, 2.0);
    pn.param("lfw", lfw, 0.13);
    pn.param("lrv", lrv, 10.0);

    //Controller parameter
    pn.param("controller_freq", controller_freq, 20);
    pn.param("AngleGain_P", Angle_gain_P, -1.0);
    pn.param("AngleGain_D", Angle_gain_D, -1.0);
    pn.param("GasGain", Gas_gain, 1.0);
    pn.param("baseSpeed", baseSpeed, 3.0);
    pn.param("baseAngle", baseAngle, 90.0);
    pn.param("forword_point", forword_point, 50);
    pn.param("test_rate", test_rate, 0.5);
    pn.param("min_speed", min_speed,3.0);

    //Publishers and Subscribers
    goal_reached_sub = n_.subscribe("/amcl_pose", 1, &L1Controller::goalReachingCB, this);
    odom_sub = n_.subscribe("/odometry/filtered", 1, &L1Controller::odomCB, this);
    path_sub = n_.subscribe("/move_base_node/NavfnROS/plan", 1, &L1Controller::pathCB, this);
    goal_sub = n_.subscribe("/move_base_simple/goal", 1, &L1Controller::goalCB, this);
    marker_pub = n_.advertise<visualization_msgs::Marker>("car_path", 10);
    pub_ = n_.advertise<geometry_msgs::Twist>("car/cmd_vel", 1);
    goal_reached_pub = n_.advertise<geometry_msgs::Point>("car/goal_reached", 1);

    //Timer
    timer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &L1Controller::controlLoopCB, this); // Duration(0.05) -> 20Hz

    //Init variables
    Lfw = goalRadius = Vcmd;
    foundForwardPt = false;
    goal_received = false;
    goal_reached = false;
    cmd_vel.linear.x = 0.0; // 0 for stop
    cmd_vel.angular.z = baseAngle;

    //Show info
    ROS_INFO("[param] baseSpeed: %f", baseSpeed);
    ROS_INFO("[param] baseAngle: %f", baseAngle);
    ROS_INFO("[param] AngleGain-P: %f", Angle_gain_P);
    ROS_INFO("[param] AngleGain-D: %f", Angle_gain_D);
    ROS_INFO ("[param] forword_point:%d", forword_point);
	ROS_INFO ("[param] min_speed:%f", min_speed);
    ROS_INFO ("[param] test_rate:%f", test_rate);
    ROS_INFO("[param] Vcmd: %f", Vcmd);
    ROS_INFO("[param] Lfw: %f", Lfw);
    

    //Visualization Marker Settings
    initMarker();
}



void L1Controller::initMarker()
{
    points.header.frame_id = line_strip.header.frame_id = goal_circle.header.frame_id = "odom";
	
    points.ns = line_strip.ns = goal_circle.ns = "Markers";
    
    points.action = line_strip.action = goal_circle.action = visualization_msgs::Marker::ADD;
    
    points.pose.orientation.w = line_strip.pose.orientation.w = goal_circle.pose.orientation.w = 1.0;
    
    points.id = 0;
    
    line_strip.id = 1;
    goal_circle.id = 2;

    points.type = visualization_msgs::Marker::POINTS;
    
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    goal_circle.type = visualization_msgs::Marker::CYLINDER;
    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.2;
    points.scale.y = 0.2;
    
    //LINE_STRIP markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;

    goal_circle.scale.x = goalRadius;
    goal_circle.scale.y = goalRadius;
    goal_circle.scale.z = 0.1;

    // Points are green
    points.color.g = 1.0f;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    //goal_circle is yellow
    goal_circle.color.r = 1.0;
    goal_circle.color.g = 1.0;
    goal_circle.color.b = 0.0;
    goal_circle.color.a = 0.5;
}

void L1Controller::goalReachingCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& goal_msg)
{
    if(goal_received)
    {
		geometry_msgs::PoseWithCovarianceStamped goal_get = *goal_msg;
		geometry_msgs::Point car_pose = goal_get.pose.pose.position;
		double car2goal_x = abs(odom_goal_pos.x - car_pose.x);
		double car2goal_y = abs(odom_goal_pos.y - car_pose.y);
		
		ROS_INFO("\nPOS_X= %.2f,POS_Y= %.2f\n",car_pose.x,car_pose.y);

        if(car2goal_x < goalRadius)
        {
			goal_reached_pub_point.x = 2.0;
			goal_reached_pub_point.y = 1.0;
			goal_reached_pub.publish(goal_reached_pub_point);
            goal_reached = true;
            goal_received = false;
            ROS_INFO("Goal Reached !");
        }
    }
}


void L1Controller::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    odom = *odomMsg;
}


void L1Controller::pathCB(const nav_msgs::Path::ConstPtr& pathMsg)
{
    map_path = *pathMsg;
}


void L1Controller::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg)
{
    try
    {
        geometry_msgs::PoseStamped odom_goal;
        tf_listener.transformPose("odom", ros::Time(0) , *goalMsg, "map" ,odom_goal);
        odom_goal_pos = odom_goal.pose.position;
        goal_received = true;
        goal_reached = false;

        /*Draw Goal on RVIZ*/
        goal_circle.pose = odom_goal.pose;
        marker_pub.publish(goal_circle);
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
}

double L1Controller::getYawFromPose(const geometry_msgs::Pose& carPose)
{
    float x = carPose.orientation.x;
    float y = carPose.orientation.y;
    float z = carPose.orientation.z;
    float w = carPose.orientation.w;

    double tmp,yaw;
    tf::Quaternion q(x,y,z,w);
    tf::Matrix3x3 quaternion(q);
    quaternion.getRPY(tmp,tmp, yaw);

    return yaw;
}

bool L1Controller::isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose)
{
    float car2wayPt_x = wayPt.x - carPose.position.x;
    float car2wayPt_y = wayPt.y - carPose.position.y;
    double car_theta = getYawFromPose(carPose);

    float car_car2wayPt_x = cos(car_theta)*car2wayPt_x + sin(car_theta)*car2wayPt_y;
    float car_car2wayPt_y = -sin(car_theta)*car2wayPt_x + cos(car_theta)*car2wayPt_y;
    
//	ROS_INFO("car_car2wayPt_x = %.2f\n",car_car2wayPt_x);

    if(car_car2wayPt_x > -1.0) /*is Forward WayPt*/
     {   return true;}
    else
    {    return false;}
}


bool L1Controller::isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos)
{
    double dx = wayPt.x - car_pos.x;
    double dy = wayPt.y - car_pos.y;
    double dist = sqrt(dx*dx + dy*dy);

    if(dist < Lfw)
        return false;
    else if(dist >= Lfw)
        return true;
}

double L1Controller::getVecAngle(const geometry_msgs::Point& Vec1, const geometry_msgs::Point& Vec2)
{
		geometry_msgs::Point c;
		double theta = atan2(Vec1.x - c.x, Vec1.y - c.y) - atan2(Vec2.x - c.x, Vec2.y - c.y);
		if (theta > PI)
			theta -= 2 * PI;
		if (theta < -PI)
			theta += 2 * PI;
	 
		theta = theta * 180.0 / PI;

		return fabs(theta);
}

geometry_msgs::Point L1Controller::get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose)
{
	double Pose_yaw = 0.0;
    geometry_msgs::Point carPose_pos = carPose.position;
    double carPose_yaw = getYawFromPose(carPose);
    geometry_msgs::Point forwardPt;
    geometry_msgs::Point forwardPt_end;
    geometry_msgs::Point odom_car2WayPtVec;
    static int exp_point = 1;
    foundForwardPt = false;
    int max_point;
    geometry_msgs::Point Vec;
	geometry_msgs::Point Vec_exp;
	geometry_msgs::PoseStamped odom_path_pose_end; //读取地图最大的点
	geometry_msgs::PoseStamped odom_path_pose;
	geometry_msgs::PoseStamped odom_path_pose_old;
	geometry_msgs::PoseStamped exp_post;

    if(!goal_reached){
				max_point = map_path.poses.size();		
				if(max_point == 0)
				{
					
				}
				else{
					ROS_INFO("maxPoint:%d\n",max_point);
					long end_point = forword_point;						//曲率检测结束点
					if(end_point>max_point) end_point = max_point;
					geometry_msgs::PoseStamped map_path_pose = map_path.poses[0];
					geometry_msgs::PoseStamped map_path_pose_end = map_path.poses[end_point];
					try{
						tf_listener.transformPose("odom", ros::Time(0) , map_path_pose, "map" ,odom_path_pose);		//起始点坐标变换
						tf_listener.transformPose("odom", ros::Time(0) , map_path_pose_end, "map" ,odom_path_pose_end);		//结束坐标点变换
					}
					catch(tf::TransformException &ex){
						ROS_ERROR("%s",ex.what());
						ros::Duration(1.0).sleep();
					}
					geometry_msgs::Point odom_path_wayPt_end = odom_path_pose_end.pose.position;
					
					Vec_exp.x = odom_path_wayPt_end.x - odom_path_pose.pose.position.x;
					Vec_exp.y = odom_path_wayPt_end.y - odom_path_pose.pose.position.y;			//期望向量
					
					try{
						for(long i = 1; i< end_point ; i++)		
						{
							map_path_pose = map_path.poses[i];
							geometry_msgs::PoseStamped map_path_pose_old = map_path.poses[i-1];
							
							tf_listener.transformPose("odom", ros::Time(0) , map_path_pose, "map" ,odom_path_pose);		//坐标变换 
							tf_listener.transformPose("odom", ros::Time(0) , map_path_pose_old, "map" ,odom_path_pose_old);		//坐标变换 
							
							if(i == exp_point) exp_post = odom_path_pose;
							
							Vec.x = odom_path_pose.pose.position.x - odom_path_pose_old.pose.position.x;
							Vec.y = odom_path_pose.pose.position.y - odom_path_pose_old.pose.position.y;	//当前向量
							
							Pose_yaw += getVecAngle(Vec,Vec_exp);						
						}
					}
					catch(tf::TransformException &ex){
						ROS_ERROR("%s",ex.what());
						ros::Duration(1.0).sleep();
					}
					Pose_yaw = Pose_yaw/end_point;
					brandrate = Pose_yaw;
					
					ROS_INFO("brandrate:%f\n",Pose_yaw);
					
					exp_point = (int)(((0.9-(Pose_yaw / 25)) * end_point)*test_rate);
					if(exp_point < 15) exp_point = 15;
					ROS_INFO("expPoint:%d\n",exp_point);
					geometry_msgs::Point odom_path_wayPt = exp_post.pose.position;			
					{
						bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt,carPose);				//目标是否在车头前面
						if(_isForwardWayPt)
						{
								forwardPt = odom_path_wayPt;
								forwardPt_end = odom_path_wayPt_end;
								foundForwardPt = true;
						}
					}
				}
        
    }
    else if(goal_reached)
    {
        forwardPt = odom_goal_pos;
        foundForwardPt = false;
        //ROS_INFO("goal REACHED!");
    }

    /*Visualized Target Point on RVIZ*/
    /*Clear former target point Marker*/
    points.points.clear();
    line_strip.points.clear();
    
    if(foundForwardPt && !goal_reached)
    {
        points.points.push_back(carPose_pos);
        points.points.push_back(forwardPt_end);
        points.points.push_back(forwardPt);
        line_strip.points.push_back(carPose_pos);
        line_strip.points.push_back(forwardPt);
    }

	marker_pub.publish(points);
    marker_pub.publish(line_strip);
    
    odom_car2WayPtVec.x = cos(carPose_yaw)*(forwardPt.x - carPose_pos.x) + sin(carPose_yaw)*(forwardPt.y - carPose_pos.y);
    odom_car2WayPtVec.y = -sin(carPose_yaw)*(forwardPt.x - carPose_pos.x) + cos(carPose_yaw)*(forwardPt.y - carPose_pos.y);
    return odom_car2WayPtVec;
}


double L1Controller::getEta(const geometry_msgs::Pose& carPose)
{
    geometry_msgs::Point odom_car2WayPtVec = get_odom_car2WayPtVec(carPose);

    double eta = atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x);
    return eta;
}

double L1Controller::getL1Distance(const double& _Vcmd)
{
    double L1 = 0;
    if(_Vcmd < 1.34)
        L1 = 3 / 3.0;
    else if(_Vcmd > 1.34 && _Vcmd < 5.36)
        L1 = _Vcmd*2.24 / 3.0;
    else
        L1 = 12 / 3.0;
    return L1;
}

double L1Controller::getSteeringAngle(double eta)
{
    double steeringAnge = -atan2((L*sin(eta)),(Lfw/2+lfw*cos(eta)))*(180.0/PI);
    //ROS_INFO("Steering Angle = %.2f", steeringAnge);
    return steeringAnge;
}

double L1Controller::getGasInput(const float& angle)
{
    double u = angle*Gas_gain;
		
    ROS_INFO("u = %.2f\n", u);
    return u;
}

void L1Controller::controlLoopCB(const ros::TimerEvent&)
{

    geometry_msgs::Pose carPose = odom.pose.pose;
    geometry_msgs::Twist carVel = odom.twist.twist;
 //   cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = baseAngle;

    if(goal_received)
    {
        /*Estimate Steering Angle*/
        double eta_last;
        double eta = getEta(carPose);  
        if(foundForwardPt)
        {
			double eta_str = getSteeringAngle(eta);
            cmd_vel.angular.z = baseAngle + (Angle_gain_P*eta_str+Angle_gain_D*(eta-eta_last));
            /*Estimate Gas Input*/
            if(!goal_reached)
            {
                double u = getGasInput(brandrate + fabs(eta_str));
				cmd_vel.linear.x = baseSpeed - u;
				if(cmd_vel.linear.x < min_speed)
				{
						cmd_vel.linear.x = min_speed;
				}
                if(cmd_vel.linear.x < 0.0) 
				{
					ROS_INFO("backward");
					cmd_vel.linear.z =  baseAngle - (Angle_gain_P*eta_str+Angle_gain_D*(eta-eta_last));
				}
               // cmd_vel.linear.x = baseSpeed;
                ROS_INFO("\nGas = %.2f\nSteering angle = %.2f",cmd_vel.linear.x,cmd_vel.angular.z);
            }
            eta_last = eta;
        }
    }
    else
    {
			cmd_vel.linear.x -= 0.05;
			if(cmd_vel.linear.x<0) cmd_vel.linear.x = 0.0;
	}
    pub_.publish(cmd_vel);
}


/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "L1Controller_v2");
    L1Controller controller;
    while(ros::ok())
    {
		ros::spin();
	}
    return 0;
}
